#include "OpenGL.h"
#include <gvars3/instances.h>
#include "Weeder.h"
#include "px.h"
#include <fstream>
#include <stdlib.h>
#include <opencv/highgui.h>
#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include <sstream>
#include <iostream>
#include <limits>
#include <numeric>
#include <algorithm>
#include <cmath>
#include "SVM.h"
#include "Bloomenthal.h"
#include "Dbscan.h"

using namespace CVD;
using namespace std;
using namespace GVars3;
using namespace TooN;
namespace config = boost::program_options;

int main(int argc, char* argv[])
{
  cout << "  Welcome to Weeder " << endl;

  string mapdirectory; 					///< The map file name
  bool silent; 					///< Whether console output should be enabled
  bool verbose; 					///< Enable verbose output
  bool debug; 					///< Enable debug functions and output
  bool train; 					///< Enable debug functions and output

	// Setup program options
	config::options_description desc("Allowed options");
	desc.add_options()
									("help", "Produce help message")
									("mapdirectory", config::value<string>(&mapdirectory)->default_value(""),	"The map directory")
									("training,t", config::bool_switch(&train)->default_value(false),	"Launch training interface (default is detection)")
									("silent,s", config::bool_switch(&silent)->default_value(false), "Surpress outputs")
									("verbose,v", config::bool_switch(&verbose)->default_value(false),"Verbose output")
									("debug,d",	config::bool_switch(&debug)->default_value(false),"Emit debug information");
	config::variables_map vm;
	config::store(config::parse_command_line(argc, argv, desc), vm);
	config::notify(vm);

	if(vm.count("help"))
  {
		std::cout << desc << std::endl;
		return 1;
	}
  else if(mapdirectory.compare("") == 0)
  {
		std::cout << "Error: No map directory specified" << std::endl;
    return 0;
  }

  GUI.LoadFile("./config/settings.cfg");
  GUI.StartParserThread();
  atexit(GUI.StopParserThread); // Clean up readline when program quits

  // Add a forward slash to the end of directory path if necessary
  unsigned int pos = mapdirectory.find_last_of("/");
  if(pos != mapdirectory.length()-1)
    mapdirectory.push_back('/');

  Weeder weed_dc(train);

  bool bLoaded = weed_dc.LoadMap(mapdirectory);

  if(!bLoaded)
  {
    cerr << "*** ERROR: Trying to load map ***" << endl;
    exit(1);
  }

  while(!weed_dc.Stop())
  {
    if(train)
      weed_dc.TrainingLoop();
    else
      weed_dc.DetectionLoop();
  }
}


Weeder::Weeder(bool bTraining)
{
  mbTraining = bTraining;
  mbDone = false;

  GV3::Register(mgvnGreylevels, "Weeder.Greylevels", 64, SILENT);
  GV3::Register(mgvnSparseSampleSpacing, "Weeder.SparseSampleSpacing", 16, SILENT);
  GV3::Register(mgvnDenseSampleSpacing, "Weeder.DenseSampleSpacing", 8, SILENT);
  GV3::Register(mgvnApproxSamplesSVM, "Weeder.ApproxSamplesSVM", 100, SILENT);
  GV3::Register(mgvdDbscanEps, "Weeder.DbscanEps", 0.1, SILENT);
  GV3::Register(mgvnDbscanMinObs, "Weeder.DbscanMinObs", 5, SILENT);
  GV3::Register(mgvnMapPointMinObs, "Weeder.MapPointMinObs", 4, SILENT);
  GV3::Register(mgvnDbscanClusterMinObs, "Weeder.DbscanClusterMinObs", 20, SILENT);
  GV3::Register(mgvdMaxScaleDepth, "Weeder.MaxScaleDepth", 4.0, SILENT);
  GV3::Register(mgvnUseApproxSVM, "Weeder.UseApproxSVM", 0, SILENT);
  GV3::Register(mgvnCameraWidth, "Camera.Width", 640, HIDDEN | FATAL_IF_NOT_DEFINED);
  GV3::Register(mgvnCameraHeight, "Camera.Height", 480, HIDDEN | FATAL_IF_NOT_DEFINED);

  GV3::Register(mgvsKeyFramesFilename, "Weeder.KeyFramesFilename", "keyframes.data", HIDDEN | FATAL_IF_NOT_DEFINED);
  GV3::Register(mgvsMapPointsFilename, "Weeder.MapPointsFilename", "mappoints.data", HIDDEN | FATAL_IF_NOT_DEFINED);
  GV3::Register(mgvsTextureScaleFilename, "Weeder.TextureScaleFilename", "texture.data", HIDDEN | FATAL_IF_NOT_DEFINED);
  GV3::Register(mgvnCurrentClass, "Weeder.CurrentClass", 0, SILENT);

  // First, check if the camera is calibrated.
  // If not, we need to run the calibration widget.
  Vector<NUMTRACKERCAMPARAMETERS> vTest = GV3::get<Vector<NUMTRACKERCAMPARAMETERS> >("Camera.Parameters", ATANCamera::mvDefaultParams, HIDDEN);
  if(vTest == ATANCamera::mvDefaultParams)
  {
    cout << endl;
    cout << "! Camera.Parameters is not set, need to run the CameraCalibrator tool" << endl;
    cout << "  and/or put the Camera.Parameters= line into the appropriate .cfg file." << endl;
    exit(1);
  }
  else
    mCamera = new ATANCamera("Camera");

  mnCameraWidth = *mgvnCameraWidth;
  mnCameraHeight = *mgvnCameraHeight;
  mImSize = cvSize(mnCameraWidth, mnCameraHeight);

  gvar3<int> gvnNumberClasses("Weeder.NumberClasses", 0, HIDDEN | FATAL_IF_NOT_DEFINED);
  string sBaseGvarName = "Weeder.ClassName";
  for(int i = 1; i <= *gvnNumberClasses; ++i)
  {
    gvar3<string> gvsClassName(sBaseGvarName + boost::lexical_cast<std::string>(i), "ClassNameX", HIDDEN | FATAL_IF_NOT_DEFINED);
    mvsClassNames.push_back(*gvsClassName);
  }
  msvmClassDetector.resize(*gvnNumberClasses, NULL);
  msvmApproxClassDetector.resize(*gvnNumberClasses, NULL);

  mColourMap = ColourMap(*gvnNumberClasses);
  mpGLWindow = new GLWindow2(ImageRef(mnCameraWidth, mnCameraHeight), "Weed detector and classification training");
  mTexGenerator = new Texture<WeederMapPoint*>(mImSize, *mgvnGreylevels);
  mBackground = cvCreateImage(mImSize, IPL_DEPTH_8U, 1);
  mPatchMask = cvCreateImage(mImSize, IPL_DEPTH_8U, 1);
  cvSetZero(mBackground);
  mnCurrentTexture = 0;

  mFeatureDim = mTexGenerator->DescriptorLengthSCARF() + mTexGenerator->DescriptorLengthVegetation();//;//mTexGenerator->DescriptorLengthColour() + 
  mTextureShift = new double[mFeatureDim];
  mTextureScale = new double[mFeatureDim];

  mse3ViewerFromWorld = SE3<>::exp(makeVector(0,0,2,0,0,0)) * SE3<>::exp(makeVector(0,0,0,0.8 * M_PI,0,0));

  GUI.RegisterCommand("quit", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.Exit", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.Reset", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.Train", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.Save", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.Detect", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.NextKeyFrame", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.NextTexture", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.RemoveKeyFrame", GUICommandCallBack, this);
  GUI.RegisterCommand("Weeder.RegenerateMap", GUICommandCallBack, this);

  GUI.ParseLine("ShowMap=0");
  GUI.ParseLine("GLWindow.AddMenu SelectionMenu Selection");
  GUI.ParseLine("SelectionMenu.AddMenuButton Root \"Save MapPoints\" Weeder.Save Root");
  GUI.ParseLine("SelectionMenu.AddMenuButton Root \"Remove Image\" Weeder.RemoveKeyFrame Root");
  GUI.ParseLine("SelectionMenu.AddMenuButton Root \"Next Image\" Weeder.NextKeyFrame Root");
  GUI.ParseLine("SelectionMenu.AddMenuButton Root \"Regenerate Map\" Weeder.RegenerateMap Root");
  GUI.ParseLine("SelectionMenu.AddMenuToggle Root \"Show Map\" ShowMap Root");
  GUI.ParseLine("SelectionMenu.ShowMenu Root");

  if(mbTraining)
  {
    GUI.ParseLine("ShowTexture=0");
    GUI.ParseLine("GLWindow.AddMenu TrainMenu Training");
    GUI.ParseLine("TrainMenu.AddMenuButton Root Reset Weeder.Reset Root");
    GUI.ParseLine("TrainMenu.AddMenuButton Root Class \"\" Class");
    for(unsigned int i = 0; i < mvsClassNames.size(); ++i)
      GUI.ParseLine("TrainMenu.AddMenuButton Class \"" + mvsClassNames[i] + "\" Weeder.CurrentClass=" + boost::lexical_cast<std::string>(i) + " Root");
    GUI.ParseLine("TrainMenu.AddMenuButton Root Train Weeder.Train Root");
    GUI.ParseLine("TrainMenu.AddMenuButton Root \"Next Texture\" Weeder.NextTexture Root");
    GUI.ParseLine("TrainMenu.AddMenuToggle Root \"Show Texture\" ShowTexture Root");
    GUI.ParseLine("TrainMenu.ShowMenu Root");
  }
  else
  {
    GUI.ParseLine("GLWindow.AddMenu DetectionMenu Detection");
    GUI.ParseLine("DetectionMenu.AddMenuButton Root Detect Weeder.Detect Root");
    GUI.ParseLine("DetectionMenu.ShowMenu Root");
  }
}

Weeder::~Weeder()
{
  for(unsigned int i = 0; i < msvmClassDetector.size(); ++i)
  {
    if(msvmClassDetector[i] != NULL)
    {
      SVM::svm_free_and_destroy_model(&msvmClassDetector[i]);
      SVM::svm_free_and_destroy_approx_model(&msvmApproxClassDetector[i]);
    }
  }
  cvReleaseImage(&mBackground);
  cvReleaseImage(&mPatchMask);

  delete [] mTextureShift;
  delete [] mTextureScale;

  delete mColourMap;
  delete mpGLWindow;
  delete mTexGenerator;

  DeleteMap();
}

bool Weeder::Stop()
{
  return mbDone;
}

void Weeder::SaveMapPoints()
{
  string sFilename = msMapDirectory + "labelled_" + *mgvsMapPointsFilename;
  ofstream ofsMapPoints(sFilename.c_str());
  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
  {
    ofsMapPoints << mvFilteredMapPoints[i]->v3WorldPos << " " << mvFilteredMapPoints[i]->v3Normal;
    ofsMapPoints << " " << mvFilteredMapPoints[i]->nClassLabel << " " << mvFilteredMapPoints[i]->nNumClasses;
    for(unsigned int c = 0; c < mvsClassNames.size(); ++c)
      ofsMapPoints << " " << mvFilteredMapPoints[i]->pdClassProb[c] << " " << mvFilteredMapPoints[i]->pnClassCount[c];
    ofsMapPoints << std::endl;
  }
  ofsMapPoints.close();
}

void Weeder::TrainingLoop()
{
  static gvar3<int> gvnShowMap("ShowMap", 0, HIDDEN|SILENT);
  static gvar3<int> gvnShowTexture("ShowTexture", 0, HIDDEN|SILENT);
  static gvar3<int> gvnShowPatches("Weeder.ShowPatches", 0, HIDDEN|SILENT);

  string caption = "Image: " + boost::lexical_cast<std::string>(distance(mvKeyFrames.begin(),mitvKeyFrames)+1) + "/" + boost::lexical_cast<std::string>(mvKeyFrames.size()) + "   Texture: " + boost::lexical_cast<std::string>(mnCurrentTexture+1) + "/" + boost::lexical_cast<std::string>(mFeatureDim) + "      ";

  // Set up openGL
  mpGLWindow->SetupViewport();
  mpGLWindow->SetupVideoOrtho();
  mpGLWindow->SetupVideoRasterPosAndZoom();
    
  WeederKeyFrame *wkf = CurrentKeyFrame();
  glRasterPos2i(0,0);
  if(wkf == NULL)
  {
    glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, mBackground->imageData);
    mpGLWindow->DrawCaption(caption + "No Images remaining");
  }
  else
  {
    if(*gvnShowMap) 
      ShowMap();
    else
    {
      glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, wkf->iplDisplay->imageData);

      vector<ImageRef> pirSelection = mpGLWindow->GetMouseSelectionUpdate();
      if(!pirSelection.empty())
      {
        CvSeq seq_header;
        CvSeqBlock block;
        cvMakeSeqHeaderForArray(CV_SEQ_ELTYPE_POINT, sizeof(seq_header), sizeof(CvPoint), &pirSelection[0], pirSelection.size(), &seq_header, &block);

        for(unsigned int i = 0; i < wkf->vKeyPoints.size(); ++i) 
        {
          if(cvPointPolygonTest(&seq_header, cvPoint2D32f(wkf->vKeyPoints[i].x, wkf->vKeyPoints[i].y), 0) > 0)
            wkf->vKeyPoints[i].data->nClassLabel = *mgvnCurrentClass;
        }
      }

      if(*gvnShowTexture)
        ShowTexture();
      if(*gvnShowPatches)
        ShowPatches();
      ShowSelections();

      mpGLWindow->DrawCaption(caption + "Select regions containing: " + mvsClassNames[*mgvnCurrentClass]);
    }
  }

  mpGLWindow->DrawMouse();
  mpGLWindow->DrawMenus();
  mpGLWindow->HandlePendingEvents();
  mpGLWindow->swap_buffers();
}


void Weeder::DetectionLoop()
{
  static gvar3<int> gvnShowMap("ShowMap", 0, HIDDEN|SILENT);
  static gvar3<int> gvnShowPatches("Weeder.ShowPatches", 0, HIDDEN|SILENT);
 
 string caption = "Image: " + boost::lexical_cast<std::string>(distance(mvKeyFrames.begin(),mitvKeyFrames)+1) + "/" + boost::lexical_cast<std::string>(mvKeyFrames.size()) + "   Texture: " + boost::lexical_cast<std::string>(mnCurrentTexture+1) + "/" + boost::lexical_cast<std::string>(mFeatureDim) + "      ";
  // Set up openGL
  mpGLWindow->SetupViewport();
  mpGLWindow->SetupVideoOrtho();
  mpGLWindow->SetupVideoRasterPosAndZoom();
    
  WeederKeyFrame *wkf = CurrentKeyFrame();
  glRasterPos2i(0,0);
  if(wkf == NULL)
  {
    glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, mBackground->imageData);
    mpGLWindow->DrawCaption(caption + "No Images remaining");
  }
  else
  {
    if(*gvnShowMap) 
      ShowMap();
    else
    {
      glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, wkf->iplDisplay->imageData);

      if(*gvnShowPatches)
        ShowPatches();

      mpGLWindow->DrawMouse();
    }
    mpGLWindow->DrawCaption(caption + "Showing weeds in: " + wkf->sFilename);
  }
  mpGLWindow->DrawMenus();
  mpGLWindow->HandlePendingEvents();
  mpGLWindow->swap_buffers();
}

void Weeder::Train()
{
  std::vector<float> vTextureMin(mFeatureDim, std::numeric_limits<double>::max());
  std::vector<float> vTextureMax(mFeatureDim, -std::numeric_limits<double>::max());

  ClearMapPointProbabilites();
  ClearKeyPointTextures();

  for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
  {
    glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, mvKeyFrames[kf]->iplDisplay->imageData);
    mpGLWindow->DrawCaption("Describing texture in current image...");
    mpGLWindow->swap_buffers();

    // Compute texture descriptors...
    std::vector<KeyPoint<WeederMapPoint*> >::iterator first = mvKeyFrames[kf]->vKeyPoints.begin();
    std::vector<KeyPoint<WeederMapPoint*> >::iterator last = mvKeyFrames[kf]->vKeyPoints.end();
    std::vector<KeyPoint<WeederMapPoint*> >::iterator bound = std::partition(first, last, IsLabelled);

    if(first != bound) // At least one keypoint is labelled in this keyframe
    {
      const IplImage *exg = mTexGenerator->convertRGBtoExG(mvKeyFrames[kf]->iplOriginal);
      //mTexGenerator->convertRGBtoScaledHSV(mvKeyFrames[kf]->iplOriginal);
      mTexGenerator->DescribeVegetation(exg, first, bound);
      //mTexGenerator->DescribeColour(mvKeyFrames[kf]->iplOriginal, first, bound);
      mTexGenerator->DescribeTextureSCARF(exg, first, bound);

      // Find texture min/max
      for(std::vector<KeyPoint<WeederMapPoint*> >::iterator it = first; it != bound; ++it)
      {
        if(CV_IMAGE_ELEM(exg, uchar, it->y, it->x) == 0)
          it->vTexture.clear();

        if(!it->vTexture.empty()) // Some points near the edge of the image will be ignored
        {
          for(unsigned int j = 0; j < it->vTexture.size(); ++j)
          {
            if(it->vTexture[j] > vTextureMax[j])
              vTextureMax[j] = it->vTexture[j];
            else if(it->vTexture[j] < vTextureMin[j])
              vTextureMin[j] = it->vTexture[j];
          }
        }
      }
    }
  }

  // Save texture shift/scale information (needed for detection)
  string sTextureScaleFilename = msMapDirectory + *mgvsTextureScaleFilename;
  ofstream ofsTextureScaleFile(sTextureScaleFilename.c_str());
  ofsTextureScaleFile << mFeatureDim << std::endl;
  for(unsigned int i = 0; i < vTextureMax.size(); ++i)
  {
    //std::cout << vTextureMin[i] << " " << vTextureMax[i] << std::endl;
    //mTextureScale[i] = (vTextureMax[i] - vTextureMin[i])/2.0;
    //mTextureShift[i] = vTextureMin[i] + mTextureScale[i];
    //mTextureScale[i] = 128;
    //mTextureShift[i] = 128;
    mTextureScale[i] = 1.0;
    mTextureShift[i] = 0.0;

    ofsTextureScaleFile << mTextureScale[i] << " " << mTextureShift[i] << " ";
  }
  ofsTextureScaleFile.close();


  // Destroy old svm classifiers if necessary
  for(unsigned int i = 0; i < msvmClassDetector.size(); ++i)
  {
    if(msvmClassDetector[i] != NULL)
    {
      SVM::svm_free_and_destroy_model(&msvmClassDetector[i]);
      msvmClassDetector[i] = NULL;
      SVM::svm_free_and_destroy_approx_model(&msvmApproxClassDetector[i]);
      msvmApproxClassDetector[i] = NULL;
    }
  }

  // Save mappoint training data to file...
  SaveMapPoints();

  ofstream ofsClassFiles[mvsClassNames.size()];
  for(unsigned int i = 0; i < mvsClassNames.size(); ++i)
  {
    string sClassFilename = msMapDirectory + mvsClassNames[i] + ".dat";
    ofsClassFiles[i].open(sClassFilename.c_str());
  }
  //Write positive training data to file as libSVM ignores training labels and expects positive examples first (or at least on the first line?)
  for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
  {
    std::vector<KeyPoint<WeederMapPoint*> >::iterator first = mvKeyFrames[kf]->vKeyPoints.begin();
    std::vector<KeyPoint<WeederMapPoint*> >::iterator last = mvKeyFrames[kf]->vKeyPoints.end();

    for(std::vector<KeyPoint<WeederMapPoint*> >::iterator it = first; it != last; ++it)
    {
      if(!it->vTexture.empty() && it->data->nClassLabel >= 0)
      {
        //label <index1>:<value1> <index2>:<value2> ... with indicies starting from 1
        ofsClassFiles[it->data->nClassLabel] << "+1";
        for(unsigned int j = 0; j < it->vTexture.size(); ++j)
          if(it->vTexture[j] != 0)
            ofsClassFiles[it->data->nClassLabel] << " " << j+1 << ":" << (it->vTexture[j]-mTextureShift[j])/mTextureScale[j];
        ofsClassFiles[it->data->nClassLabel] << std::endl;
      }
    }
  }

  //Write negative training data to file as libSVM ignores training labels and expects positive examples first (or at least on the first line?)
  for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
  {
    std::vector<KeyPoint<WeederMapPoint*> >::iterator first = mvKeyFrames[kf]->vKeyPoints.begin();
    std::vector<KeyPoint<WeederMapPoint*> >::iterator last = mvKeyFrames[kf]->vKeyPoints.end();

    for(std::vector<KeyPoint<WeederMapPoint*> >::iterator it = first; it != last; ++it)
    {
      if(!it->vTexture.empty() && it->data->nClassLabel >= 0)
      {
        for(int c = 0; c < (int)mvsClassNames.size(); ++c)
        {
          if(c != it->data->nClassLabel)
          {
            //label <index1>:<value1> <index2>:<value2> ... with indicies starting from 1
            ofsClassFiles[c] << "-1";
            for(unsigned int j = 0; j < it->vTexture.size(); ++j)
              if(it->vTexture[j] != 0)
                ofsClassFiles[c] << " " << j+1 << ":" << (it->vTexture[j]-mTextureShift[j])/mTextureScale[j];
            ofsClassFiles[c] << std::endl;
          }
        }
      }
    }
  }

  WeederKeyFrame *wkf = CurrentKeyFrame();
  for(unsigned int i = 0; i < mvsClassNames.size(); ++i)
  {
    ofsClassFiles[i].close(); // Close the training data file..

    if(wkf == NULL)
      glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, mBackground->imageData);
    else
      glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, wkf->iplDisplay->imageData);
    mpGLWindow->DrawCaption("Training one-vs-all SVM for " + mvsClassNames[i]);
    mpGLWindow->swap_buffers();

    struct SVM::svm_problem* classprob = SVM::svm_read_problem((msMapDirectory + mvsClassNames[i] + ".dat").c_str());

    // Set parameter values
    struct SVM::svm_parameter param;
	  param.type = SVM::C_SVC;
	  param.kernel_type = SVM::INTERSECTION;//SVM::INTERSECTION;//
	  param.degree = 3;
	  param.gamma = 1.0/classprob->feature_dim;
    param.feature_dim = classprob->feature_dim;
    param.samples = *mgvnApproxSamplesSVM;
	  param.coef0 = 0;
	  param.nu = 0.5;
	  param.cache_size = 100;
	  param.C = 1;
	  param.eps = 1e-3;
	  param.p = 0.1;
	  param.shrinking = 1;
	  param.probability = 1;
	  param.nr_weight = 0;
	  param.weight_label = NULL;
	  param.weight = NULL;

	  const char *error_msg = SVM::svm_check_parameter(classprob, &param);
	  if(error_msg)
	  {
      if(wkf == NULL)
        glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, mBackground->imageData);
      else
        glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, wkf->iplDisplay->imageData);
      mpGLWindow->DrawCaption("Error: " + string(error_msg));
      mpGLWindow->swap_buffers();
	  }
    else
    {
	    msvmClassDetector[i] = SVM::svm_train(classprob, &param);
      SVM::svm_save_model((msMapDirectory + mvsClassNames[i] + ".model").c_str(), msvmClassDetector[i]);
      msvmApproxClassDetector[i] = SVM::svm_create_approx_model(msvmClassDetector[i], &param, true);
      SVM::svm_save_approx_model((msMapDirectory + mvsClassNames[i] + ".apxmodel").c_str(), msvmApproxClassDetector[i]);
    }
    SVM::svm_free_and_destroy_problem(&classprob);
  }
}

void Weeder::Detect()
{
  struct SVM::svm_node *desc = (struct SVM::svm_node*)malloc((mFeatureDim+1)*sizeof(struct SVM::svm_node));
  for(unsigned int i = 0; i < mFeatureDim; ++i)
    desc[i].index = i+1;
  desc[mFeatureDim].index = -1; // Terminating index, must be present

  ClearMapPointProbabilites();
  ClearKeyPointTextures();

  for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
  {
    glDrawPixels(mnCameraWidth, mnCameraHeight, GL_BGR, GL_UNSIGNED_BYTE, mvKeyFrames[kf]->iplDisplay->imageData);
    mpGLWindow->DrawCaption("Classifying texture in current image...");
    mpGLWindow->swap_buffers();

    std::vector<KeyPoint<WeederMapPoint*> >::iterator first = mvKeyFrames[kf]->vKeyPoints.begin();
    std::vector<KeyPoint<WeederMapPoint*> >::iterator last = mvKeyFrames[kf]->vKeyPoints.end();

    const IplImage* exg = mTexGenerator->convertRGBtoExG(mvKeyFrames[kf]->iplOriginal);
    mTexGenerator->DescribeVegetation(exg, first, last);
    //mTexGenerator->DescribeColour(mvKeyFrames[kf]->iplOriginal, first, last);
    mTexGenerator->DescribeTextureSCARF(exg, first, last);


    std::string sPointsFilename = "points_" + boost::lexical_cast<std::string>(kf) + ".jpg";
    ofstream ofsPoints(sPointsFilename.c_str());
    for(std::vector<KeyPoint<WeederMapPoint*> >::iterator it = first; it != last; ++it)
    {
      if(CV_IMAGE_ELEM(exg, uchar, it->y, it->x) == 0)
        it->vTexture.clear();

      if(!it->vTexture.empty()) // Some points near the edge of the image will be ignored
      {
        ofsPoints << it->x << " " << it->y << std::endl;

        //Convert descriptor to libSVM format and normalize
        for(unsigned int j = 0; j < mFeatureDim; ++j)
        {
          if(mTextureScale[j] != 0)
            desc[j].value = (it->vTexture[j]-mTextureShift[j])/mTextureScale[j];
          else
            desc[j].value = 0.0;
        }

        for(unsigned int c = 0; c < msvmClassDetector.size(); ++c) //TODO: use classifier 0 (grass)?
        {
          double label, prob_estimates[2];

          if(*mgvnUseApproxSVM)
            label = SVM::svm_predict_approx_probability(msvmApproxClassDetector[c], desc, prob_estimates);
          else
            label = SVM::svm_predict_probability(msvmClassDetector[c], desc, prob_estimates);

          //if((c == 0 && label < 0) || (c > 0 && label > 0))
          if(label > 0)
          {
            it->data->pdClassProb[c] += prob_estimates[0];
            it->data->pnClassCount[c]++;
          }
          /*else
          {
            it->data->pdClassProb[0] += prob_estimates[1];
            it->data->pnClassCount[0]++;
          }*/
        }
      }
    }
    ofsPoints.close();
  }

  ShowDetections();
  EvaluateDetectionAccuracy();
  SaveMapPoints();

  free(desc);
}

void Weeder::DeleteMap()
{
  for(unsigned int i = 0; i < mvKeyFrames.size(); ++i)
    delete mvKeyFrames[i];
  mvKeyFrames.clear();
  for(unsigned int i = 0; i < mvMapPoints.size(); ++i)
    delete mvMapPoints[i];
  mvMapPoints.clear();
}

void Weeder::Reset()
{
  //DeleteMap();
  //LoadMap(msMapDirectory);
  //ClearMapPoints();
  //ClearKeyPoints();
}

void Weeder::ClearMapPointLabels()
{
  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
    mvFilteredMapPoints[i]->nClassLabel = -1;
}

void Weeder::ClearMapPointProbabilites()
{
  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
  {
    memset(mvFilteredMapPoints[i]->pdClassProb, 0, mvsClassNames.size()*sizeof(double));
    memset(mvFilteredMapPoints[i]->pnClassCount, 0, mvsClassNames.size()*sizeof(int));
  }
}

void Weeder::ClearKeyPointTextures()
{
  for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
  {
    for(unsigned int i = 0; i < mvKeyFrames[kf]->vKeyPoints.size(); ++i)
      mvKeyFrames[kf]->vKeyPoints[i].vTexture.clear();
  }
}

bool Weeder::LoadMap(const std::string sMapDirectory)
{
  string sImageFilename, sNewLine;
  TooN::Vector<3> v3WorldPos;
  msMapDirectory = sMapDirectory;

  if(!mbTraining)
  {
    for(unsigned int i = 0; i < msvmClassDetector.size(); ++i)
    {
	    msvmClassDetector[i] = SVM::svm_load_model((msMapDirectory + mvsClassNames[i] + ".model").c_str());
	    msvmApproxClassDetector[i] = SVM::svm_load_approx_model((msMapDirectory + mvsClassNames[i] + ".apxmodel").c_str());
      if(msvmClassDetector[i] == NULL || msvmApproxClassDetector[i] == NULL)
      {
        cerr << "Error: Directory does not contained trained SVM" << endl;
        return false;
      }
    }
  }

  string sTextureScalePath = msMapDirectory + *mgvsTextureScaleFilename;
  ifstream ifsTextureScaleFile(sTextureScalePath.c_str());
  if(ifsTextureScaleFile.is_open())
  {
    ifsTextureScaleFile >> mFeatureDim;
    if(mFeatureDim != (mTexGenerator->DescriptorLengthSCARF() + mTexGenerator->DescriptorLengthVegetation()))//mTexGenerator->DescriptorLengthColour() + (
    {
      cerr << "Error: Training texture dimension does not match texture generator dimension" << endl;
      return false;
    }

    for(unsigned int i = 0; i < mFeatureDim; ++i)
    {
      ifsTextureScaleFile >> mTextureScale[i];
      ifsTextureScaleFile >> mTextureShift[i];
    }
    ifsTextureScaleFile.close();
  }

  string sKeyFramesPath = msMapDirectory + *mgvsKeyFramesFilename;
  ifstream ifsKeyFrames(sKeyFramesPath.c_str());
  if(!ifsKeyFrames.is_open())
  {
    cerr << "Error: Unable to open keyframe datafile" << endl;
    return false;
  }

  std::cout << "Loading Key Frames..." << std::endl;
  while(ifsKeyFrames >> sImageFilename) 
  {
    std::cout << "Loading " << sImageFilename << std::endl;
    WeederKeyFrame *wkf = new WeederKeyFrame(mvsClassNames.size());
    wkf->iplOriginal = cvLoadImage(sImageFilename.c_str(), CV_LOAD_IMAGE_COLOR);
    if(wkf->iplOriginal == NULL)
    {
      cerr << "Error: Unable to keyframe RGB image" << endl;
      return false;
    }
    //wkf->iplDisplay = cvCloneImage(wkf->iplOriginal);
    wkf->iplDisplay = cvLoadImage(sImageFilename.c_str(), CV_LOAD_IMAGE_COLOR);
    wkf->GenerateTexture();

    //int pos = sImageFilename.find_last_of(".");
    //wkf->sFilename = sImageFilename.substr(0, pos);
    wkf->sFilename = sImageFilename;

    ifsKeyFrames >> wkf->se3CfromW;

    mvKeyFrames.push_back(wkf);
  }
  mitvKeyFrames = mvKeyFrames.begin();
  ifsKeyFrames.close();
  FilterKeyFrames();

  string sLabelledMapPointsFilename = msMapDirectory + "labelled_" + *mgvsMapPointsFilename;
  ifstream ifsMapPoints(sLabelledMapPointsFilename.c_str());

  if(!ifsMapPoints.is_open())
  {
    string sMapPointsFilename = msMapDirectory + *mgvsMapPointsFilename;
    ifsMapPoints.open(sMapPointsFilename.c_str());
    if(!ifsMapPoints.is_open())
    {
      cerr << "Error: Unable to open mappoints datafile" << endl;
      return false;
    }

    std::cout << "Loading Raw Map Points..." << std::endl;
    int nNumObservations;
    while(ifsMapPoints >> v3WorldPos) 
    {
      WeederMapPoint *mpt = new WeederMapPoint(mvsClassNames.size());

      mpt->v3WorldPos = v3WorldPos;
      ifsMapPoints >> mpt->v3Normal;
      ifsMapPoints >> nNumObservations;

      if(nNumObservations < *mgvnMapPointMinObs)
        delete mpt;
      else
        mvMapPoints.push_back(mpt);
    }
    ifsMapPoints.close();
    FilterMapPoints();
  }
  else
  {
    std::cout << "Loading Previous Session Map Points..." << std::endl;
    while(ifsMapPoints >> v3WorldPos) 
    {
      WeederMapPoint *mpt = new WeederMapPoint(mvsClassNames.size());

      mpt->v3WorldPos = v3WorldPos;
      ifsMapPoints >> mpt->v3Normal;
      ifsMapPoints >> mpt->nClassLabel;
      ifsMapPoints >> mpt->nNumClasses;
      for(unsigned int i = 0; i < mvsClassNames.size(); ++i)
      {
        ifsMapPoints >> mpt->pdClassProb[i];
        ifsMapPoints >> mpt->pnClassCount[i];
      }
      mvMapPoints.push_back(mpt);
    }
    ifsMapPoints.close();
    mvFilteredMapPoints = mvMapPoints;
  }

  Vector<2> v2Image;
  double depth;
  for(unsigned int i = 0; i < mvKeyFrames.size(); ++i)
  {
    for(unsigned int j = 0; j < mvFilteredMapPoints.size(); ++j)
    {
      if(Project(mvKeyFrames[i]->se3CfromW, mvFilteredMapPoints[j]->v3WorldPos, v2Image, depth))
      {
       //Compute angle between point normal and keyframe optical axis
        Vector<3> v3CamNormal = mvKeyFrames[i]->se3CfromW.inverse().get_rotation() * makeVector(0,0,1);
        double angle = acos(mvFilteredMapPoints[j]->v3Normal * v3CamNormal); 
        if(angle < 2.3562)
          continue;

        int scale = (int)(*mgvdMaxScaleDepth/depth + 0.5);
        if(scale < 1 || scale > 4)
          continue;

        mvKeyFrames[i]->vKeyPoints.push_back(KeyPoint<WeederMapPoint*>((int)v2Image[0], (int)v2Image[1], scale, mvFilteredMapPoints[j]));
      }
    }
  }

  if(!mbTraining)
    ShowDetections();

  std::cout << "Computing 3D terrain model..." << std::endl;
  ComputeMesh();

  std::cout << "Done." << std::endl;
  return true;
}

void Weeder::ShowSelections()
{
  vector<KeyPoint<WeederMapPoint*> > &vKeyPoints = CurrentKeyFrame()->vKeyPoints;

  glPointSize(3); glBegin(GL_POINTS); glEnable(GL_POINT_SMOOTH);
  for(unsigned int i = 0; i < vKeyPoints.size(); ++i)
  {
    // Black if not any class
    glColor3f(0,0,0);
    for(int c = 0; c < (int)mvsClassNames.size(); ++c)
      if(vKeyPoints[i].data->nClassLabel == c)
        glColor3f(mColourMap[c].val[0]/255.0,mColourMap[c].val[1]/255.0,mColourMap[c].val[2]/255.0);

    glVertex2i(vKeyPoints[i].x, vKeyPoints[i].y);
  }
  glEnd(); glDisable(GL_POINT_SMOOTH);
}

void Weeder::ShowDetections()
{
  // Generate detection display images...
  for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
  {
    cvCopy(mvKeyFrames[kf]->iplOriginal, mvKeyFrames[kf]->iplDisplay);
    for(std::vector<KeyPoint<WeederMapPoint*> >::iterator it = mvKeyFrames[kf]->vKeyPoints.begin(); it != mvKeyFrames[kf]->vKeyPoints.end(); ++it)
    {
      double max_prob = 0.0, prob;
      int max_index = 0;
      for(unsigned int c = 0; c < msvmClassDetector.size(); ++c)
      {
        if(it->data->pnClassCount[c] > 0)
        {
          prob = it->data->pdClassProb[c]/it->data->pnClassCount[c];
          if(prob > max_prob)
          {
            max_prob = prob;
            max_index = c;
          }
        }
      }
      //std::cout << it->data->pdClassProb[0] << " " << it->data->pnClassCount[0] << " " << it->data->pdClassProb[1] << " " << it->data->pnClassCount[1] << std::endl;

      if(max_index != 0 && max_prob > 0.5)
        DrawCircle(mvKeyFrames[kf]->iplDisplay, cvPoint(it->x, it->y), mTexGenerator->Offset(), mColourMap[max_index], 1.0-max_prob/2, max_prob/2);
    }
    mvKeyFrames[kf]->GenerateTexture();
  }
}

void Weeder::EvaluateDetectionAccuracy()
{
  int nPositives = 0, nNegatives = 0;
  int nTruePositive = 0, nFalsePostive = 0, nTrueNegative = 0, nFalseNegative = 0, nUnclassified = 0;
  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
  {
    if(mvFilteredMapPoints[i]->nClassLabel >= 0) //Ignore any unlabelled points
    {
      // Find most likely class for this mappoint given detection data
      double max_prob = 0.0, prob;
      int max_index = -1;
      for(unsigned int c = 0; c < msvmClassDetector.size(); ++c)
      {
        if(mvFilteredMapPoints[i]->pnClassCount[c] > 0)
        {
          prob = mvFilteredMapPoints[i]->pdClassProb[c]/mvFilteredMapPoints[i]->pnClassCount[c];
          if(prob > max_prob)
          {
            max_prob = prob;
            max_index = c;
          }
        }
      }
      
      if(mvFilteredMapPoints[i]->nClassLabel == 0)
        nNegatives++;
      else
        nPositives++;

      if(max_index < 0) // Unclassified
        nUnclassified++;
      else if(max_index > 0 && mvFilteredMapPoints[i]->nClassLabel > 0) // Correctly identified weed
        nTruePositive++;
      else if(max_index == mvFilteredMapPoints[i]->nClassLabel) // Correctly identified grass
        nTrueNegative++;
      else if(max_index > 0 && mvFilteredMapPoints[i]->nClassLabel == 0) // Incorrectly identified grass as weed
        nFalsePostive++;
      else if(max_index == 0 && mvFilteredMapPoints[i]->nClassLabel > 0) // Incorrectly identified weed as grass
        nFalseNegative++;
    }
  }

  std::cout << "Detection Accuracy: " << std::endl;
  std::cout << "True Positive: " << 100.0*nTruePositive/nPositives << " %" << std::endl;
  std::cout << "False Negative: " << 100.0*nFalseNegative/nPositives << " %" << std::endl;
  std::cout << "True Negative: " << 100.0*nTrueNegative/nNegatives << " %" << std::endl;
  std::cout << "False Positive: " << 100.0*nFalsePostive/nNegatives << " %" << std::endl;
  std::cout << "Unclassified: " << 100.0*nUnclassified/(nPositives+nNegatives) << " %" << std::endl;
  std::cout << "Number of samples: " << (nPositives+nNegatives) << std::endl;
}

void Weeder::ShowMap()
{
  static gvar3<int> gvnShowMapMesh("Weeder.ShowMapMesh", 1, SILENT);
  static gvar3<int> gvnShowMapKeyFrames("Weeder.ShowMapKeyFrames", 1, SILENT);

  // Update viewer position according to mouse input:
  pair<Vector<6>, Vector<6> > pv6 = mpGLWindow->GetMousePoseUpdate();
  SE3<> se3CamFromMC;
  se3CamFromMC.get_translation() = mse3ViewerFromWorld * mv3MassCenter;
  mse3ViewerFromWorld = SE3<>::exp(pv6.first) * se3CamFromMC * SE3<>::exp(pv6.second) * se3CamFromMC.inverse() * mse3ViewerFromWorld;

  mpGLWindow->SetupViewport();
  glClearColor(0,0,0,0);
  glClearDepth(1);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glEnable(GL_POINT_SMOOTH);
  glEnable(GL_DEPTH_TEST);
  //glEnable(GL_LINE_SMOOTH);
  //glEnable(GL_BLEND);
  //glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  //glColorMask(1,1,1,1);

  //ShowMapDots();
  if(*gvnShowMapMesh)
    ShowMesh();
  if(*gvnShowMapKeyFrames)
    for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
      ShowKeyFramePose(mvKeyFrames[kf]);

  glDisable(GL_POINT_SMOOTH);
  glDisable(GL_DEPTH_TEST);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}

void Weeder::SetupFrustum()
{
  glMatrixMode(GL_PROJECTION);  
  glLoadIdentity();
  double zNear = 0.03;
  glFrustum(-zNear, zNear, 0.75*zNear,-0.75*zNear,zNear,50);
  glScalef(1,1,-1);
  return;
}

void Weeder::SetupModelView(SE3<> se3WorldFromCurrent)
{
  glMatrixMode(GL_MODELVIEW);  
  glLoadIdentity();
  glMultMatrix(mse3ViewerFromWorld * se3WorldFromCurrent);
  return;
}

/*void Weeder::ShowMapDots()
{
  SetupFrustum();
  SetupModelView();

  int nPointSize = mTexGenerator->WindowSize();
  int nForMass = 0;
  glColor3f(1,0,0);
  glPointSize(1);
  //glEnable(GL_BLEND);
  glEnable(GL_POINT_SMOOTH);
  //glBlendFunc(GL_ONE, GL_ONE);
  glBegin(GL_POINTS);
  mv3MassCenter = Zeros;

  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
  {
    Vector<3> &v3Pos = mvFilteredMapPoints[i]->v3WorldPos; 
    if(v3Pos * v3Pos < 10000)
    {
      nForMass++;
      mv3MassCenter += v3Pos;
    }

    if(mvFilteredMapPoints[i]->nClassLabel == *mgvnCurrentClass) // Red if current class
    {
      double red = mvFilteredMapPoints[i]->pdClassProb[*mgvnCurrentClass]/mvFilteredMapPoints[i]->pnClassCount[*mgvnCurrentClass];
      glColor3f(red,0,0);
      glVertex(v3Pos);
    }
  }
  glEnd();
  glDisable(GL_POINT_SMOOTH);
  //glDisable(GL_BLEND);
  mv3MassCenter = mv3MassCenter / (0.1 + nForMass);
}*/

void Weeder::ShowMesh()
{
  SetupFrustum();
  SetupModelView();

  glEnable(GL_LINE_SMOOTH);
  glEnable(GL_TEXTURE_2D);

  // the texture is clamped at the edges
  glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP );
  glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP );
  glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);

  std::vector<boost::shared_ptr<Cube> > cubes;
  unsigned int cube_N = mesh.GetAllCubes(cubes);
  Vector<2> v2ImageSize = mCamera->GetImageSize();

  for(unsigned int i = 0; i < cube_N; ++i)
  {
    for(unsigned int j = 0; j < cubes[i]->faces.size(); ++j)
    {
      glBindTexture(GL_TEXTURE_2D, cubes[i]->faces[j]->patches[0]->gluTextureId);
      glBegin(GL_TRIANGLES);
      glTexCoord2d(cubes[i]->faces[j]->patches[0]->x1/v2ImageSize[0], cubes[i]->faces[j]->patches[0]->y1/v2ImageSize[1]); 
      glVertex3d(cubes[i]->faces[j]->x1, cubes[i]->faces[j]->y1, cubes[i]->faces[j]->z1);
      glTexCoord2d(cubes[i]->faces[j]->patches[0]->x2/v2ImageSize[0], cubes[i]->faces[j]->patches[0]->y2/v2ImageSize[1]); 
      glVertex3d(cubes[i]->faces[j]->x2, cubes[i]->faces[j]->y2, cubes[i]->faces[j]->z2);
      glTexCoord2d(cubes[i]->faces[j]->patches[0]->x3/v2ImageSize[0], cubes[i]->faces[j]->patches[0]->y3/v2ImageSize[1]); 
      glVertex3d(cubes[i]->faces[j]->x3, cubes[i]->faces[j]->y3, cubes[i]->faces[j]->z3);
      glEnd();
    }
  }

  glDisable(GL_TEXTURE_2D);
  glDisable(GL_LINE_SMOOTH);
}

void Weeder::ShowKeyFramePose(WeederKeyFrame *kf)
{
  SetupModelView(kf->se3CfromW.inverse());
  SetupFrustum();
  
  glLineWidth(3);
  
  glBegin(GL_LINES);
  glColor3f(1,0,0);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.1f, 0.0f, 0.0f);
  glColor3f(0,1,0);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.0f, 0.1f, 0.0f);
  glColor3f(1,1,1);
  glVertex3f(0.0f, 0.0f, 0.0f);
  glVertex3f(0.0f, 0.0f, 0.1f);
  glEnd();
}

void Weeder::ShowPatches()
{
  WeederKeyFrame *wkf = CurrentKeyFrame();

  std::vector<boost::shared_ptr<Cube> > cubes;
  unsigned int cube_N = mesh.GetAllCubes(cubes);

  glColor4f(1.0,1.0,1.0,1.0);
  glLineWidth(1);

  for(unsigned int i = 0; i < cube_N; ++i)
  {
    for(unsigned int j = 0; j < cubes[i]->faces.size(); ++j)
    {
      for(unsigned int k = 0; k < cubes[i]->faces[j]->patches.size(); ++k)
      {
        if((WeederKeyFrame*)(cubes[i]->faces[j]->patches[k]->data) == wkf)
        {
          glBegin(GL_LINE_STRIP);
          glVertex2i(cubes[i]->faces[j]->patches[k]->x1, cubes[i]->faces[j]->patches[k]->y1);
          glVertex2i(cubes[i]->faces[j]->patches[k]->x2, cubes[i]->faces[j]->patches[k]->y2);
          glVertex2i(cubes[i]->faces[j]->patches[k]->x3, cubes[i]->faces[j]->patches[k]->y3);
          glVertex2i(cubes[i]->faces[j]->patches[k]->x1, cubes[i]->faces[j]->patches[k]->y1);
          glEnd();
        }
      }
    }
  }
}

void Weeder::ShowTexture()
{
  vector<KeyPoint<WeederMapPoint*> > &vKeyPoints = CurrentKeyFrame()->vKeyPoints;
  int nPointSize = mTexGenerator->WindowSize();
  glEnable(GL_BLEND);
  glEnable(GL_POINT_SMOOTH);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glPointSize(nPointSize); glBegin(GL_POINTS);
  for(unsigned int i = 0; i < vKeyPoints.size(); ++i)
  {
    if(!vKeyPoints[i].vTexture.empty())
    {
      double alpha = (vKeyPoints[i].vTexture[mnCurrentTexture]-mTextureShift[mnCurrentTexture])/mTextureScale[mnCurrentTexture];
      glColor4f(0,0,1,alpha);
      glVertex2i(vKeyPoints[i].x, vKeyPoints[i].y);
    }
  }
  glEnd();
  glDisable(GL_POINT_SMOOTH);
  glDisable(GL_BLEND);
}

void Weeder::NextTextureLoop()
{
  mnCurrentTexture = (mnCurrentTexture + 1) % mFeatureDim;
}

WeederKeyFrame* Weeder::CurrentKeyFrame()
{
  if(mitvKeyFrames == mvKeyFrames.end())
    return NULL;
  else
    return *mitvKeyFrames;
}

WeederKeyFrame* Weeder::NextKeyFrameLoop()
{
  if(mitvKeyFrames == mvKeyFrames.end()) // Only when no keyframes left...
    return NULL;
  else if(++mitvKeyFrames == mvKeyFrames.end())
  {
    mitvKeyFrames = mvKeyFrames.begin();
    if(mitvKeyFrames == mvKeyFrames.end())
      return NULL;
    else
    {
      mse3ViewerFromWorld = (*mitvKeyFrames)->se3CfromW;
      return *mitvKeyFrames;
    }
  }
  else
  {
    mse3ViewerFromWorld = (*mitvKeyFrames)->se3CfromW;
    return *mitvKeyFrames;
  }
}

WeederKeyFrame* Weeder::RemoveKeyFrame()
{
  if(mitvKeyFrames == mvKeyFrames.end())
    return NULL;
  else
  {
    std::cout << "Removing keyframe: " << (*mitvKeyFrames)->sFilename << std::endl;

    delete *mitvKeyFrames;
    mitvKeyFrames = mvKeyFrames.erase(mitvKeyFrames);

    if(mitvKeyFrames == mvKeyFrames.end())
    {
      mitvKeyFrames = mvKeyFrames.begin();
      if(mitvKeyFrames == mvKeyFrames.end())
        return NULL;
      else
      {
        mse3ViewerFromWorld = (*mitvKeyFrames)->se3CfromW;
        return *mitvKeyFrames;
      }
    }
    else
    {
      mse3ViewerFromWorld = (*mitvKeyFrames)->se3CfromW;
      return *mitvKeyFrames;
    }
  }
}

void Weeder::GUICommandCallBack(void* ptr, string sCommand, string sParams)
{
  ((Weeder*) ptr)->GUICommandHandler(sCommand, sParams);
}

void Weeder::GUICommandHandler(string sCommand, string sParams)  // Called by the callback func..
{
  if(sCommand=="Weeder.Reset")
  {
    Reset();
    return;
  }
  if(sCommand=="Weeder.NextKeyFrame")
  {
    NextKeyFrameLoop();
  }
  if(sCommand=="Weeder.RemoveKeyFrame")
  {
    RemoveKeyFrame();
  }
  if(sCommand=="Weeder.RegenerateMap")
  {
    ComputeMesh();
  }
  if(sCommand=="Weeder.NextTexture")
  {
    NextTextureLoop();
  }
  if(sCommand=="Weeder.Train")
  {
    Train();
  }
  if(sCommand=="Weeder.Save")
  {
    SaveMapPoints();
  }
  if(sCommand=="Weeder.Detect")
  {
    Detect();
  }
  if(sCommand=="Weeder.Exit" || sCommand=="quit")
  {
    mbDone = true;
  }
}

int demo()
{
  // Loop over kernel types and train models
  const int kernels[] = {5, 6, 7}; 
  for(unsigned int k = 0; k < 1; ++k) //TODO: fix malloc error and restore to 3..
  {
	  struct SVM::svm_problem* prob_train = SVM::svm_read_problem("demosvm.data");//demosvm.data

    // Set parameter values
    struct SVM::svm_parameter param;
	  param.type = SVM::C_SVC;
	  param.kernel_type = kernels[k];
	  param.degree = 3;
	  param.gamma = 1.0/prob_train->feature_dim;
    param.feature_dim = prob_train->feature_dim;
    param.samples = 100;
	  param.coef0 = 0;
	  param.nu = 0.5;
	  param.cache_size = 100;
	  param.C = 1;
	  param.eps = 1e-3;
	  param.p = 0.1;
	  param.shrinking = 1;
	  param.probability = 1; //default for libSVM is 0;
	  param.nr_weight = 0;
	  param.weight_label = NULL;
	  param.weight = NULL;

	  const char *error_msg = SVM::svm_check_parameter(prob_train, &param);
	  if(error_msg)
	  {
		  fprintf(stderr,"Error: %s\n",error_msg);
		  exit(1);
	  }

	  struct SVM::svm_model *model = SVM::svm_train(prob_train, &param);
    struct SVM::svm_approx_model *approxmodel1 = SVM::svm_create_approx_model(model, &param, false); 
    struct SVM::svm_approx_model *approxmodel2 = SVM::svm_create_approx_model(model, &param, true); // BINARY SEARCH works only for intersection kernels

    // Check if the models are similar (should be!)
    double max_error = 0.0;
    for(int i = 0; i < model->param.feature_dim; ++i)
    {
      for(int j = 0; j < model->param.samples; ++j)
      {
        double error = fabs(approxmodel1->fa[i][j] - approxmodel2->fa[i][j]);
        if(error > max_error)
          max_error = error;
      }
    }
    std::cout << "Error: " << max_error << std::endl;

    SVM::svm_save_approx_model("model1.data", approxmodel1);
    struct SVM::svm_approx_model* loaded1 = SVM::svm_load_approx_model("model1.data");

    // Predict the class/probability
	  struct SVM::svm_problem* prob_test = SVM::svm_read_problem("demosvm.data");
    double prob_estimates[model->nr_class], prob_estimates1[model->nr_class], prob_estimates2[model->nr_class];
    double dec_values[model->nr_class-1], dec_values1[model->nr_class-1], dec_values2[model->nr_class-1], dec_values3[model->nr_class-1];
    double error1 = 0.0, error2 = 0.0, error3 = 0.0, perror1 = 0.0, perror2 = 0.0;
    int misclassified1 = 0, misclassified2 = 0, misclassified3 = 0;
    double label, label1, label2, label3;
    const double epsilon = 1e-8;
    for(int i = 0; i < prob_test->l; ++i)
    {
      label = SVM::svm_predict_values(model, prob_test->x[i], dec_values);
      label1 = SVM::svm_predict_approx_values(approxmodel1, prob_test->x[i], dec_values1);
      label2 = SVM::svm_predict_approx_values(approxmodel2, prob_test->x[i], dec_values2);
      label3 = SVM::svm_predict_approx_values(loaded1, prob_test->x[i], dec_values3);

      if(fabs(label - label1) > epsilon)
        misclassified1++;
      if(fabs(label - label2) > epsilon)
        misclassified2++;
      if(fabs(label - label3) > epsilon)
        misclassified3++;

      error1 += fabs(dec_values[0] - dec_values1[0]);
      error2 += fabs(dec_values[0] - dec_values2[0]);
      error3 += fabs(dec_values[0] - dec_values3[0]);

      label = SVM::svm_predict_probability(model, prob_test->x[i], prob_estimates);
      label1 = SVM::svm_predict_approx_probability(approxmodel1, prob_test->x[i], prob_estimates1);
      label2 = SVM::svm_predict_approx_probability(approxmodel2, prob_test->x[i], prob_estimates2);

      perror1 += fabs(prob_estimates[0] - prob_estimates1[0]);
      perror2 += fabs(prob_estimates[0] - prob_estimates2[0]);
    }
    std::cout << "Approx model 1: " << error1/prob_test->l << " " << perror1/prob_test->l << " " << misclassified1 << std::endl;
    std::cout << "Approx model 2: " << error2/prob_test->l << " " << perror2/prob_test->l << " " << misclassified2 << std::endl;
    std::cout << "Loaded model 1: " << error3/prob_test->l << " " << "N/A" << " " << misclassified3 << std::endl;


	  SVM::svm_free_and_destroy_model(&model);
	  SVM::svm_free_and_destroy_approx_model(&loaded1);
	  SVM::svm_free_and_destroy_approx_model(&approxmodel1);
	  SVM::svm_free_and_destroy_approx_model(&approxmodel2);
    SVM::svm_destroy_param(&param);
    SVM::svm_free_and_destroy_problem(&prob_train);
    SVM::svm_free_and_destroy_problem(&prob_test);
  }

	return 0;
}

void Weeder::ComputeMesh()
{
  static gvar3<double> gvdMinPatchAngle("Weeder.MinPatchAngle", 2.3562, SILENT); //135 degrees
  static gvar3<int> gvnMinPatchArea("Weeder.MinPatchArea", 100, SILENT);  //Projected patch must have an area of at least 100 pixels
  static gvar3<int> gvnMinPointsRBF("Weeder.MinPointsRBF", 100, SILENT);
  static gvar3<int> gvnMinPointsBloomenthal("Weeder.MinPointsBloomenthal", 50, SILENT);//100
  static gvar3<double> gvdSmoothRBF("Weeder.SmoothRBF", 0.2, SILENT);//0.2
  static gvar3<double> gvdErrorRBF("Weeder.ErrorRBF", 0.1, SILENT);//0.1

  //double dLargestRadiusInImage = mCamera->LargestRadiusInImage() * mCamera->LargestRadiusInImage();
  Vector<2> v2ImageSize = mCamera->GetImageSize();
  PointSet* ps = new PointSet();
  ps->allocateStorage(mvFilteredMapPoints.size()); //Allocate maximum storage

  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
  {
    ps->addPoint(mvFilteredMapPoints[i]->v3WorldPos[0], mvFilteredMapPoints[i]->v3WorldPos[1], mvFilteredMapPoints[i]->v3WorldPos[2], 
                -mvFilteredMapPoints[i]->v3Normal[0], -mvFilteredMapPoints[i]->v3Normal[1], -mvFilteredMapPoints[i]->v3Normal[2]);
  }

  if(ps->getNumberPoints() > *gvnMinPointsRBF)
  {  
    double dGridSize = mesh.GetGridSize();
    RBF *rbf = MultiScaleRBF(ps, *gvdSmoothRBF, *gvdErrorRBF);
    rbf->setSupportConstraint(*gvnMinPointsBloomenthal);

	  Bloomenthal bloom;
    Bloomenthal::VERTICES vertexlist;
    Bloomenthal::CUBELIST* cubelist;
    double dOriginX = rbf->getPointSet()->getPointX(0);
    double dOriginY = rbf->getPointSet()->getPointY(0);
    double dOriginZ = rbf->getPointSet()->getPointZ(0);

	  bloom.setImplicitFunction(rbf);
    int nCubeCount = bloom.polygonize(vertexlist, cubelist, dOriginX, dOriginY, dOriginZ, dGridSize);

    // Offset of this mesh relative to existing mesh with origin at (0,0,0)
    int nOriginCi = floor(dOriginX/dGridSize);
    int nOriginCj = floor(dOriginY/dGridSize);
    int nOriginCk = floor(dOriginZ/dGridSize);

    mesh.Reset(); //TODO: Do not reset!!!

    //Pass through insert new mesh faces
	  Bloomenthal::CUBELIST *ccube = cubelist;
	  for(int i = 0; i < nCubeCount; i++)
    {
      boost::shared_ptr<Cube> newcube(new Cube());

      Bloomenthal::TRILIST *ctri = ccube->tris;
      for(int j = 0; j < ccube->triscount; j++)
      {
        Vector<3> v3Vertex0 = makeVector(vertexlist.ptr[ctri->i1].position.x, vertexlist.ptr[ctri->i1].position.y, vertexlist.ptr[ctri->i1].position.z);
        Vector<3> v3Vertex1 = makeVector(vertexlist.ptr[ctri->i2].position.x, vertexlist.ptr[ctri->i2].position.y, vertexlist.ptr[ctri->i2].position.z);
        Vector<3> v3Vertex2 = makeVector(vertexlist.ptr[ctri->i3].position.x, vertexlist.ptr[ctri->i3].position.y, vertexlist.ptr[ctri->i3].position.z);
        Vector<3> v3Normal = unit((v3Vertex1 - v3Vertex0) ^ (v3Vertex2 - v3Vertex0));  //outward pointing normal
        Vector<3> v3Center = makeVector((v3Vertex0[0]+v3Vertex1[0]+v3Vertex2[0])/3,(v3Vertex0[1]+v3Vertex1[1]+v3Vertex2[1])/3,(v3Vertex0[2]+v3Vertex1[2]+v3Vertex2[2])/3);
       
        newcube->faces.push_back(new Face(vertexlist.ptr[ctri->i1].position.x, vertexlist.ptr[ctri->i1].position.y, vertexlist.ptr[ctri->i1].position.z,
        vertexlist.ptr[ctri->i2].position.x, vertexlist.ptr[ctri->i2].position.y, vertexlist.ptr[ctri->i2].position.z,
        vertexlist.ptr[ctri->i3].position.x, vertexlist.ptr[ctri->i3].position.y, vertexlist.ptr[ctri->i3].position.z, 
        ctri->b1, ctri->b2, ctri->b3, 
        v3Center[0], v3Center[1], v3Center[2],
        v3Normal[0], v3Normal[1], v3Normal[2]));

        for(unsigned int kf = 0; kf < mvKeyFrames.size(); ++kf)
        {
          Vector<2> v2ImagePix0, v2ImagePix1, v2ImagePix2;
          double depth;

          if( !Project(mvKeyFrames[kf]->se3CfromW, v3Vertex0, v2ImagePix0, depth) || 
              !Project(mvKeyFrames[kf]->se3CfromW, v3Vertex1, v2ImagePix1, depth) || 
              !Project(mvKeyFrames[kf]->se3CfromW, v3Vertex2, v2ImagePix2, depth) )
            continue;

          //Compute angle between patch normal and keyframe optical axis
          /*Vector<3> v3CamNormal = mvKeyFrames[kf]->se3CfromW.inverse().get_rotation() * makeVector(0,0,1);
          double angle = acos(v3Normal * v3CamNormal); 
          if(angle < *gvdMinPatchAngle)
            continue;*/

        /*double area = 0.5*fabs((v2ImagePix1[0]-v2ImagePix0[0])*(v2ImagePix2[1]-v2ImagePix0[1])-(v2ImagePix2[0]-v2ImagePix0[0])*(v2ImagePix1[1]-v2ImagePix0[1]));
          if(area < *gvnMinPatchArea)
            continue;*/

          newcube->faces.back()->patches.push_back(new Patch(mvKeyFrames[kf]->gluTextureId, v2ImagePix0[0], v2ImagePix0[1], v2ImagePix1[0], v2ImagePix1[1], v2ImagePix2[0], v2ImagePix2[1], (void*)mvKeyFrames[kf]));
          break;  //TODO: Only really need one patch?
        }

        //Check face has at least one patch...
        if(newcube->faces.back()->patches.size() == 0)
          newcube->faces.pop_back();

        ctri = ctri->next;
	    }

      //Check cube has at least one face...
      if(newcube->faces.size() == 0)
        newcube.reset();
      else
        mesh.SetCube(nOriginCi+ccube->ci, nOriginCj+ccube->cj, nOriginCk+ccube->ck, newcube);

      ccube = ccube->next;
    }

    ccube = cubelist;
	  for(int i = 0; i < nCubeCount; i++)
    {
      Bloomenthal::TRILIST *ctri = ccube->tris;
      for(int j = 0; j < ccube->triscount; j++)
      {
		    Bloomenthal::TRILIST* previous = ctri;
		    ctri = ctri->next;
		    delete previous;
      }
      Bloomenthal::CUBELIST* pcube = ccube;
      ccube = ccube->next;
      delete pcube;
    }

	  if (vertexlist.ptr != NULL) 
      free((char *)vertexlist.ptr);

    delete rbf;
  }

  delete ps;
}

bool Weeder::comp(float a, float b)
{
  if (fabs(a - b) < 0.00001)
    return true;
  else
    return false;
}

RBF* Weeder::MultiScaleRBF(PointSet *ps, const float smooth, const float error) 
{
	//Estimation of Final Level
  RBF *rbf = new RBF;
	rbf->setPointSet(ps, false);
	float peak = rbf->getOctreePeak(); 
	int M = -(int)(log(rbf->getOctreeAveragedLeafSize()*2.0/(peak*3.0))/log(2.0)) + 1;
	float support = 1.5f*peak;
	float final = support*pow(2.0, -M);

	//increase child 
	rbf->splitOctreeChild(M);

	//assign cell points by averaging
	rbf->computeOctreeCenter();

	RBF** rbfi = new RBF*[M+1];
	PointSet** qs = new PointSet*[M+1];

	for(int k=0; k<=M; k++)
  {
		int size;
    if(k == M)
      size = ps->getNumberPoints();
    else
      rbf->generateOctreePointList(size, 0, 0, 0, 1000000, k);//10000000000

		qs[k] = new PointSet;
		qs[k]->allocateStorage(size);

    if(k == M)
    {
      if(error != 0)
      {
        //int dropped = 0;
		    for(int i=0; i < size; i++)
        {
			    float g[3], p[3] = {ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i)};
			    float f = rbfi[k-1]->valueAndGradient(g, p[0], p[1], p[2]);
			    float len = sqrt(g[0]*g[0] + g[1]*g[1] + g[2]*g[2]);
			    float dist; 
			    if(len != 0)
				    dist = fabs(f)/len;
			    else
				    dist = 10000000;

			    if(dist < final*error && (1.0 - (ps->getNormalX(i)*g[0] + ps->getNormalY(i)*g[1] + ps->getNormalZ(i)*g[2]))/len < 0.05)
				    continue;//dropped++;//
			
			    /*if(len != 0){
				    if(f > 0){
					    p[0] -= final*error*g[0]/len;
					    p[1] -= final*error*g[1]/len;
					    p[2] -= final*error*g[2]/len;
				    }
				    else{
					    p[0] += final*error*g[0]/len;
					    p[1] += final*error*g[1]/len;
					    p[2] += final*error*g[2]/len;
				    }
			    }*/

		      qs[k]->addPoint(p[0], p[1], p[2], ps->getNormalX(i), ps->getNormalY(i), ps->getNormalZ(i));
		    }
        //std::cout << M << " dropped: " << dropped << "/" << size << std::endl;
	    }
      else
      {
	      for(int i=0; i<size; i++)
		      qs[k]->addPoint(ps->getPointX(i), ps->getPointY(i), ps->getPointZ(i), ps->getNormalX(i), ps->getNormalY(i), ps->getNormalZ(i));
      }
    }
    else
    {
			if(error != 0 && k != 0)
      {
        //int dropped = 0;
				for(int i=0; i<size; i++)
        {
					float g[3];
					float f = rbfi[k-1]->valueAndGradient(g, rbf->getOctreePointX(i), rbf->getOctreePointY(i), rbf->getOctreePointZ(i));
					float len = sqrt(g[0]*g[0] + g[1]*g[1] + g[2]*g[2]);
					float dist; 
					if(len != 0)
						dist = (fabs(f)/len);
					else
						dist = 10000000;

					if(dist < final*error && (1.0 - (rbf->getOctreeNormalX(i)*g[0] + rbf->getOctreeNormalY(i)*g[1] + rbf->getOctreeNormalZ(i)*g[2]))/len < 0.05)
						continue;//dropped++;//

					qs[k]->addPoint(rbf->getOctreePointX(i), rbf->getOctreePointY(i), rbf->getOctreePointZ(i), 
                          rbf->getOctreeNormalX(i), rbf->getOctreeNormalY(i), rbf->getOctreeNormalZ(i));
				}
        //std::cout << k << " dropped: " << dropped << "/" << size << std::endl;
			}
      else
      {
		    for(int i=0; i<size; i++)
        {
			    qs[k]->addPoint(rbf->getOctreePointX(i), rbf->getOctreePointY(i), rbf->getOctreePointZ(i),
			                    rbf->getOctreeNormalX(i), rbf->getOctreeNormalY(i), rbf->getOctreeNormalZ(i));
		    }
      }
    }

		rbfi[k] = new RBF;
		rbfi[k]->setPointSet(qs[k], true);

    if(k == 0)
    {
			rbfi[k]->setSupport(support);
			rbfi[k]->setGlobal();
    }
		else
    {
	    rbfi[k]->setSubRBF(rbfi[k-1]);
	    rbfi[k]->computeQuad(support); //Local fitting
      rbfi[k]->fit(support, 0.0f,smooth*k); 			//Global fitting
		}
		support *= 0.5f;
	}

  delete rbf; //delete original rbf

  return rbfi[M];
}

void Weeder::FilterMapPoints()
{
  std::vector<std::vector<WeederMapPoint*> > vClusters;
  DBSCAN<WeederMapPoint> dbscan(*mgvdDbscanEps, *mgvnDbscanMinObs);
  dbscan.Cluster(mvMapPoints, vClusters);

  for(unsigned int i = 0; i < vClusters.size(); ++i)
  {
    if((int)vClusters[i].size() > *mgvnDbscanClusterMinObs)
    {
      for(unsigned int j = 0; j < vClusters[i].size(); ++j)
        if(!vClusters[i][j]->IsUnAssigned() && !vClusters[i][j]->IsNoise())
          mvFilteredMapPoints.push_back(vClusters[i][j]);
    }
  }
  // Remove any duplicates (points belonging to multiple clusters)
  std::sort(mvFilteredMapPoints.begin(), mvFilteredMapPoints.end());
  mvFilteredMapPoints.erase(std::unique(mvFilteredMapPoints.begin(), mvFilteredMapPoints.end()), mvFilteredMapPoints.end());
  std::random_shuffle(mvFilteredMapPoints.begin(), mvFilteredMapPoints.end());  // Not sure why, but mesh making fails on sorted set?

  string sFilename = msMapDirectory + "test.dat";
  ofstream ofsTest(sFilename.c_str());
  for(unsigned int i = 0; i < mvFilteredMapPoints.size(); ++i)
  {
    ofsTest << mvFilteredMapPoints[i] << std::endl;
  }
  ofsTest.close();
}

void Weeder::FilterKeyFrames()
{
  std::vector<WeederKeyFrame*>::iterator it = mvKeyFrames.begin();
  while(it != mvKeyFrames.end())
  {
    //Compute angle between optical axis and vertical
    if(acos(makeVector(0,0,-1) * (*it)->se3CfromW.inverse().get_rotation() * makeVector(0,0,1)) > 0.26)
      it = mvKeyFrames.erase(it);
    else
      ++it;
  }
}

bool Weeder::Project(const SE3<> &se3CFromW, const Vector<3> &v3World, Vector<2> &v2Image, double &depth)
{
  Vector<3> v3Cam = se3CFromW * v3World;
  if(v3Cam[2] < 0.001)
    return false;

  Vector<2> v2ImPlane = project(v3Cam);
  if(v2ImPlane*v2ImPlane > mCamera->LargestRadiusInImage() * mCamera->LargestRadiusInImage())
    return false;

  v2Image = mCamera->Project(v2ImPlane);
  if(mCamera->Invalid())
    return false;
  
  if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > mCamera->GetImageSize()[0] || v2Image[1] > mCamera->GetImageSize()[1])
    return false;
  else
  {
    depth = v3Cam[2];
    return true;
  }
}

CvScalar* Weeder::ColourMap(int size)
{
  const CvScalar colourMap[256] = {{{255,0,0,0}},{{255,5.97656250000000,0,0}},{{255,11.9531250000000,0,0}},{{255,17.9296875000000,0,0}},{{255,23.9062500000000,0,0}},{{255,29.8828125000000,0,0}},{{255,35.8593750000000,0,0}},{{255,41.8359375000000,0,0}},{{255,47.8125000000000,0,0}},{{255,53.7890625000000,0,0}},{{255,59.7656250000000,0,0}},{{255,65.7421875000000,0,0}},{{255,71.7187500000000,0,0}},{{255,77.6953125000000,0,0}},{{255,83.6718750000000,0,0}},{{255,89.6484375000000,0,0}},{{255,95.6250000000000,0,0}},{{255,101.601562500000,0,0}},{{255,107.578125000000,0,0}},{{255,113.554687500000,0,0}},{{255,119.531250000000,0,0}},{{255,125.507812500000,0,0}},{{255,131.484375000000,0,0}},{{255,137.460937500000,0,0}},{{255,143.437500000000,0,0}},{{255,149.414062500000,0,0}},{{255,155.390625000000,0,0}},{{255,161.367187500000,0,0}},{{255,167.343750000000,0,0}},{{255,173.320312500000,0,0}},{{255,179.296875000000,0,0}},{{255,185.273437500000,0,0}},{{255,191.250000000000,0,0}},{{255,197.226562500000,0,0}},{{255,203.203125000000,0,0}},{{255,209.179687500000,0,0}},{{255,215.156250000000,0,0}},{{255,221.132812500000,0,0}},{{255,227.109375000000,0,0}},{{255,233.085937500000,0,0}},{{255,239.062500000000,0,0}},{{255,245.039062500000,0,0}},{{255,251.015625000000,0,0}},{{253.007812500000,255,0,0}},{{247.031250000000,255,0,0}},{{241.054687500000,255,0,0}},{{235.078125000000,255,0,0}},{{229.101562500000,255,0,0}},{{223.125000000000,255,0,0}},{{217.148437500000,255,0,0}},{{211.171875000000,255,0,0}},{{205.195312500000,255,0,0}},{{199.218750000000,255,0,0}},{{193.242187500000,255,0,0}},{{187.265625000000,255,0,0}},{{181.289062500000,255,0,0}},{{175.312500000000,255,0,0}},{{169.335937500000,255,0,0}},{{163.359375000000,255,0,0}},{{157.382812500000,255,0,0}},{{151.406250000000,255,0,0}},{{145.429687500000,255,0,0}},{{139.453125000000,255,0,0}},{{133.476562500000,255,0,0}},{{127.500000000000,255,0,0}},{{121.523437500000,255,0,0}},{{115.546875000000,255,0,0}},{{109.570312500000,255,0,0}},{{103.593750000000,255,0,0}},{{97.6171875000000,255,0,0}},{{91.6406250000000,255,0,0}},{{85.6640625000000,255,0,0}},{{79.6875000000000,255,0,0}},{{73.7109375000000,255,0,0}},{{67.7343750000000,255,0,0}},{{61.7578125000000,255,0,0}},{{55.7812500000000,255,0,0}},{{49.8046875000000,255,0,0}},{{43.8281250000000,255,0,0}},{{37.8515625000000,255,0,0}},{{31.8750000000000,255,0,0}},{{25.8984375000000,255,0,0}},{{19.9218750000000,255,0,0}},{{13.9453125000000,255,0,0}},{{7.96875000000000,255,0,0}},{{1.99218750000000,255,0,0}},{{0,255,3.98437500000000,0}},{{0,255,9.96093750000000,0}},{{0,255,15.9375000000000,0}},{{0,255,21.9140625000000,0}},{{0,255,27.8906250000000,0}},{{0,255,33.8671875000000,0}},{{0,255,39.8437500000000,0}},{{0,255,45.8203125000000,0}},{{0,255,51.7968750000000,0}},{{0,255,57.7734375000000,0}},{{0,255,63.7500000000000,0}},{{0,255,69.7265625000000,0}},{{0,255,75.7031250000000,0}},{{0,255,81.6796875000000,0}},{{0,255,87.6562500000000,0}},{{0,255,93.6328125000000,0}},{{0,255,99.6093750000000,0}},{{0,255,105.585937500000,0}},{{0,255,111.562500000000,0}},{{0,255,117.539062500000,0}},{{0,255,123.515625000000,0}},{{0,255,129.492187500000,0}},{{0,255,135.468750000000,0}},{{0,255,141.445312500000,0}},{{0,255,147.421875000000,0}},{{0,255,153.398437500000,0}},{{0,255,159.375000000000,0}},{{0,255,165.351562500000,0}},{{0,255,171.328125000000,0}},{{0,255,177.304687500000,0}},{{0,255,183.281250000000,0}},{{0,255,189.257812500000,0}},{{0,255,195.234375000000,0}},{{0,255,201.210937500000,0}},{{0,255,207.187500000000,0}},{{0,255,213.164062500000,0}},{{0,255,219.140625000000,0}},{{0,255,225.117187500000,0}},{{0,255,231.093750000000,0}},{{0,255,237.070312500000,0}},{{0,255,243.046875000000,0}},{{0,255,249.023437500000,0}},{{0,255,255,0}},{{0,249.023437500000,255,0}},{{0,243.046875000000,255,0}},{{0,237.070312500000,255,0}},{{0,231.093750000000,255,0}},{{0,225.117187500000,255,0}},{{0,219.140625000000,255,0}},{{0,213.164062500000,255,0}},{{0,207.187500000000,255,0}},{{0,201.210937500000,255,0}},{{0,195.234375000000,255,0}},{{0,189.257812500000,255,0}},{{0,183.281250000000,255,0}},{{0,177.304687500000,255,0}},{{0,171.328125000000,255,0}},{{0,165.351562500000,255,0}},{{0,159.375000000000,255,0}},{{0,153.398437500000,255,0}},{{0,147.421875000000,255,0}},{{0,141.445312500000,255,0}},{{0,135.468750000000,255,0}},{{0,129.492187500000,255,0}},{{0,123.515625000000,255,0}},{{0,117.539062500000,255,0}},{{0,111.562500000000,255,0}},{{0,105.585937500000,255,0}},{{0,99.6093750000000,255,0}},{{0,93.6328125000000,255,0}},{{0,87.6562500000000,255,0}},{{0,81.6796875000000,255,0}},{{0,75.7031250000000,255,0}},{{0,69.7265625000000,255,0}},{{0,63.7500000000000,255,0}},{{0,57.7734375000000,255,0}},{{0,51.7968750000000,255,0}},{{0,45.8203125000000,255,0}},{{0,39.8437500000000,255,0}},{{0,33.8671875000000,255,0}},{{0,27.8906250000000,255,0}},{{0,21.9140625000000,255,0}},{{0,15.9375000000000,255,0}},{{0,9.96093750000000,255,0}},{{0,3.98437500000000,255,0}},{{1.99218750000000,0,255,0}},{{7.96875000000000,0,255,0}},{{13.9453125000000,0,255,0}},{{19.9218750000000,0,255,0}},{{25.8984375000000,0,255,0}},{{31.8750000000000,0,255,0}},{{37.8515625000000,0,255,0}},{{43.8281250000000,0,255,0}},{{49.8046875000000,0,255,0}},{{55.7812500000000,0,255,0}},{{61.7578125000000,0,255,0}},{{67.7343750000000,0,255,0}},{{73.7109375000000,0,255,0}},{{79.6875000000000,0,255,0}},{{85.6640625000000,0,255,0}},{{91.6406250000000,0,255,0}},{{97.6171875000000,0,255,0}},{{103.593750000000,0,255,0}},{{109.570312500000,0,255,0}},{{115.546875000000,0,255,0}},{{121.523437500000,0,255,0}},{{127.500000000000,0,255,0}},{{133.476562500000,0,255,0}},{{139.453125000000,0,255,0}},{{145.429687500000,0,255,0}},{{151.406250000000,0,255,0}},{{157.382812500000,0,255,0}},{{163.359375000000,0,255,0}},{{169.335937500000,0,255,0}},{{175.312500000000,0,255,0}},{{181.289062500000,0,255,0}},{{187.265625000000,0,255,0}},{{193.242187500000,0,255,0}},{{199.218750000000,0,255,0}},{{205.195312500000,0,255,0}},{{211.171875000000,0,255,0}},{{217.148437500000,0,255,0}},{{223.125000000000,0,255,0}},{{229.101562500000,0,255,0}},{{235.078125000000,0,255,0}},{{241.054687500000,0,255,0}},{{247.031250000000,0,255,0}},{{253.007812500000,0,255,0}},{{255,0,251.015625000000,0}},{{255,0,245.039062500000,0}},{{255,0,239.062500000000,0}},{{255,0,233.085937500000,0}},{{255,0,227.109375000000,0}},{{255,0,221.132812500000,0}},{{255,0,215.156250000000,0}},{{255,0,209.179687500000,0}},{{255,0,203.203125000000,0}},{{255,0,197.226562500000,0}},{{255,0,191.250000000000,0}},{{255,0,185.273437500000,0}},{{255,0,179.296875000000,0}},{{255,0,173.320312500000,0}},{{255,0,167.343750000000,0}},{{255,0,161.367187500000,0}},{{255,0,155.390625000000,0}},{{255,0,149.414062500000,0}},{{255,0,143.437500000000,0}},{{255,0,137.460937500000,0}},{{255,0,131.484375000000,0}},{{255,0,125.507812500000,0}},{{255,0,119.531250000000,0}},{{255,0,113.554687500000,0}},{{255,0,107.578125000000,0}},{{255,0,101.601562500000,0}},{{255,0,95.6250000000000,0}},{{255,0,89.6484375000000,0}},{{255,0,83.6718750000000,0}},{{255,0,77.6953125000000,0}},{{255,0,71.7187500000000,0}},{{255,0,65.7421875000000,0}},{{255,0,59.7656250000000,0}},{{255,0,53.7890625000000,0}},{{255,0,47.8125000000000,0}},{{255,0,41.8359375000000,0}},{{255,0,35.8593750000000,0}},{{255,0,29.8828125000000,0}},{{255,0,23.9062500000000,0}},{{255,0,17.9296875000000,0}},{{255,0,11.9531250000000,0}},{{255,0,5.97656250000000,0}}};

  CvScalar *cmap = new CvScalar[size];
  int step = (int)floor(256.0/size);
  for(int i = 0; i < size; ++i)
    cmap[i] = colourMap[i*step];
  return cmap;
}

void Weeder::DrawCircle(IplImage *src, CvPoint center, int radius, CvScalar colour, double alpha, double beta)
{
  const CvSize size = cvGetSize(src);
  const size_t step = src->widthStep;
  const int channels = src->nChannels;
  uchar* ptr = (uchar*)src->imageData;

  if(channels == 3) //Opencv has BGR format, colour specified in RGB format.
    swap(colour.val[0], colour.val[2]);

  if(center.x >= radius && center.x < size.width - radius && center.y >= radius && center.y < size.height - radius) // whole circle inside
  {
    // Draw centerline
    uchar *pRowPtrCenter = ptr + center.y * step + (center.x-radius)*channels;
    uchar *pRowPtrCenterEnd = pRowPtrCenter + (2*radius*channels);
    for( ; pRowPtrCenter <= pRowPtrCenterEnd; pRowPtrCenter += (channels))
      for(int hline_j = 0; hline_j < (channels); hline_j++ )
        pRowPtrCenter[hline_j] = (uchar)(alpha*pRowPtrCenter[hline_j] + beta*colour.val[hline_j]);

    for(int yoffset = 1; yoffset < radius; ++yoffset)
    {
      int xoffset = sqrt(radius*radius - yoffset*yoffset);
      int xstart = center.x-xoffset;

      uchar *pRowPtrUp = ptr + (center.y - yoffset) * step + xstart*channels;
      uchar *pRowPtrUpEnd = pRowPtrUp + (2*xoffset*channels);
      for( ; pRowPtrUp <= pRowPtrUpEnd; pRowPtrUp += (channels))
        for(int hline_j = 0; hline_j < (channels); hline_j++ )
          pRowPtrUp[hline_j] = (uchar)(alpha*pRowPtrUp[hline_j] + beta*colour.val[hline_j]);

      uchar *pRowPtrDown = ptr + (center.y + yoffset) * step + xstart*channels;
      uchar *pRowPtrDownEnd = pRowPtrDown + (2*xoffset*channels);
      for( ; pRowPtrDown <= pRowPtrDownEnd; pRowPtrDown += (channels))
        for(int hline_j = 0; hline_j < (channels); hline_j++ )
          pRowPtrDown[hline_j] = (uchar)(alpha*pRowPtrDown[hline_j] + beta*colour.val[hline_j]);
    }
  }
  else if(center.x >= 0 && center.x < size.width && center.y >= 0 && center.y < size.height) // center inside
  {
    // Draw centerline
    uchar *pRowPtrCenter = ptr + center.y * step;
    uchar *pRowPtrCenterStart = pRowPtrCenter + (max(center.x-radius,0)*channels);
    uchar *pRowPtrCenterEnd = pRowPtrCenter + (min(center.x+radius,size.width-1)*channels);
    for( ; pRowPtrCenterStart <= pRowPtrCenterEnd; pRowPtrCenterStart += (channels))
      for(int hline_j = 0; hline_j < (channels); hline_j++ )
        pRowPtrCenterStart[hline_j] = (uchar)(alpha*pRowPtrCenterStart[hline_j] + beta*colour.val[hline_j]);

    for(int yoffset = 1; yoffset < radius; ++yoffset)
    {
      int xoffset = sqrt(radius*radius - yoffset*yoffset);  
      int xstart = max(center.x-xoffset,0);
      int xend = min(center.x+xoffset,size.width-1);

      if(center.y - yoffset >= 0)
      {
        uchar *pRowPtrUp = ptr + (center.y - yoffset) * step;
        uchar *pRowPtrUpStart = pRowPtrUp + xstart*channels;
        uchar *pRowPtrUpEnd = pRowPtrUp + xend*channels;
        for( ; pRowPtrUpStart <= pRowPtrUpEnd; pRowPtrUpStart += (channels))
          for(int hline_j = 0; hline_j < (channels); hline_j++ )
            pRowPtrUpStart[hline_j] = (uchar)(alpha*pRowPtrUpStart[hline_j] + beta*colour.val[hline_j]);
      }

      if(center.y + yoffset < size.height)
      {
        uchar *pRowPtrDown = ptr + (center.y + yoffset) * step;
        uchar *pRowPtrDownStart = pRowPtrDown + xstart*channels;
        uchar *pRowPtrDownEnd = pRowPtrDown + xend*channels;
        for( ; pRowPtrDownStart <= pRowPtrDownEnd; pRowPtrDownStart += (channels))
          for(int hline_j = 0; hline_j < (channels); hline_j++ )
            pRowPtrDownStart[hline_j] = (uchar)(alpha*pRowPtrDownStart[hline_j] + beta*colour.val[hline_j]);
      }
    }
  }
}
